#include <math.h>
#include "PID.h"

//定义转向pid参数结构体
_PID TurnPdate={
        .kp=14,			//赋值比例值
        .ki=0.1,			//赋值积分值
        .kd=3		//赋值微分值
};
//定义速度pid参数结构体
_PID SpdPdate={
        .kp=-50,			//赋值比例值-80
        .ki=-6,			//赋值积分值-10
        .kd=0			//赋值微分值
};
//定义角度pid参数结构体
_PID AnglePdate={
        .kp=1.8,			//赋值比例值
        .ki=0.001,			//赋值积分值
        .kd=0.5		//赋值微分值
};
/*@brief: 位置式PID控制器
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
float Bias, Pwm, Integral_bias, Last_Bias=0;

float PositionPID(float deviation) {
    float Position_KP = TurnPdate.kp, Position_KI = TurnPdate.ki, Position_KD = TurnPdate.kd;
    static float last_pwm;
    Bias = deviation;                                         //计算偏差
    Integral_bias += Bias;                                     //求出偏差的积分
    Pwm = Position_KP * Bias + Position_KI * Integral_bias + Position_KD * (Bias - Last_Bias); //位置式PID控制器

    Last_Bias = Bias;                                         //保存上一次偏差


    last_pwm=Pwm;
    return Pwm;
}
void PositionPID_Clear()
{
    Bias, Pwm, Integral_bias, Last_Bias=0;
}


/*@brief: 角度式PID控制器
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
float AnglePID(float deviation)
{
    float Position_KP=AnglePdate.kp,Position_KI=AnglePdate.ki,Position_KD=AnglePdate.kd;
    static float Bias,angle,Integral_bias,Last_Bias;
    Bias=deviation;                         		         //计算偏差
    Integral_bias+=Bias;	                                 //求出偏差的积分
    angle=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
    Last_Bias=Bias;                                      	 //保存上一次偏差
    return angle;
}
float currentYaw, error, targetYaw, integral, derivative, previousError=0;

float PID_CascadeCalc(float input) {

    currentYaw = input;
    error = targetYaw - currentYaw;
    integral += error;  // dt为采样时间间隔
    derivative = (error - previousError);  // previousError为上一次的偏差
    previousError = error;

    return AnglePdate.kp * error + AnglePdate.ki * integral + AnglePdate.kd * derivative;
}
void CascadeCalc_Clear()
{
    currentYaw, error, targetYaw, integral, derivative, previousError=0;
}

    /*@brief: 为速度pid使用位置式PID控制器，对积分值进行限幅
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
//int PositionPIDToSpd(float deviation,_PID pid)
//{
//    float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
//    static float Bias,Pwm,Integral_bias,Last_Bias,pwmKI=0;
//    Bias=deviation;                         		         //计算偏差
//    Integral_bias+=Bias;	                                 //求出偏差的积分
//    pwmKI=Position_KI*Integral_bias;
//    if(pwmKI>MAX_MOTOR_PWM) Integral_bias=MAX_MOTOR_PWM/Position_KI;
//    Pwm=Position_KP*Bias+pwmKI+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
//    Last_Bias=Bias;                                      	 //保存上一次偏差
//    return Pwm;
//}
